//----------------------------------------------------------------------------------------------
//	Filename:	PointCloudSceneSolution.cpp
//	Author:		Sandro Spina
//	Date:		21/06/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------

#include "PointCloudSceneSolution.h"
#include <boost\lexical_cast.hpp>

using namespace Illumina::Core;

//**********************************************
//**********Constructor*************************
PointCloudSceneSolution::PointCloudSceneSolution(PointCloudSegmentation * p_sceneToSolve, ModelTraining * p_trainedModelDB)
{				
    m_sceneToSolve = p_sceneToSolve;
    m_trainedModelDB = p_trainedModelDB;
    //Initialise the root of the Markov Decision Process Search Space.
    m_markovDecisionProcess = new SolutionSpace();
    m_markovDecisionProcess->m_root->crtActiveNodesCount = p_sceneToSolve->m_structureGraph->m_nodes->size();
    m_markovDecisionProcess->m_root->crtActiveEdgesCount = 0;
    m_markovDecisionProcess->m_root->crtActivePointsCount = 0;
    for (auto nodes_it = p_sceneToSolve->m_structureGraph->m_nodes->begin(); nodes_it != p_sceneToSolve->m_structureGraph->m_nodes->end(); nodes_it++)
    {
        m_markovDecisionProcess->m_root->crtActiveEdgesCount += nodes_it->second->edges->size();
        m_markovDecisionProcess->m_root->crtActivePointsCount += nodes_it->second->segment->points->size();
    }
    m_downsampledSegments = new std::map<int, std::vector<int>*>();
    model_original_positions = new std::map<int, std::vector<Vector3>*>();
}	

//**********************************************
//**********Search Functions********************
int PointCloudSceneSolution::ComputeSolution(void)
{
    boost::timer start_search_timer;
    std::cout << "-->Start Computing Search Space::" << std::endl;
    int fit1 = ComputeMDPSearchSpace1();  //Starting from the root		
    std::cout << "<--Done Computing Search Space::" << start_search_timer.elapsed() << std::endl;
    return 0;
}

//1. Search for boundaries; first check for floor nodes in the graph if one exists ... then check for walls and ceiling.
//2. Iterate over the most prominant segments in the scene and determine which objects (chairs, tables) fit best.
int PointCloudSceneSolution::ComputeMDPSearchSpace1()
{
    int searchSpaceSize = 0;
    m_sceneToSolve->ComputeOBB(0.0f); //Compute OBB for Scene To Solve

    StructureGraph * structureGraph = m_sceneToSolve->m_structureGraph;
    SegmentsGraph * segmentsGraph = m_sceneToSolve->m_segmentsGraph;
    
    //0. Promote internal nodes of compound segmetns to level0 from level1(internal)
    m_sceneToSolve->m_structureGraph->PromoteInternalNodesToLevel0();	

    //1. Shrink the search space first by automerging adjacent coplanar segments - THIS MIGHT PROVE TO BE PROBLEMATIC - 
    //   One possible solution is to do this after a first iteration of the searching process - 
    //   Alternatibely only merge the very small states now. Leave those that are relatively similar to what is already in the graph untouched.
    m_sceneToSolve->MergeStates(false, 0, 2, 0.95f, 0.05f, 0);
    //m_sceneToSolve->m_segmentsGraph->m_pcs->DrawSceneGraph(m_sceneToSolve->m_structureGraph, m_sceneToSolve->m_pointCloud->GetName(), 1.2f);

    //2. Determine Boundaries - If there are none (e.g. when matching a group of objects), this should be somehow specified.
    SearchBoundaries(m_markovDecisionProcess->m_root, 0.95f, 0.2f, 20.0f);
    
    //If boundaries are determined, these are attached to the root node.
    std::cout << "Nodes attached to root = " << m_markovDecisionProcess->m_root->GetNumberOfChildren() << std::endl;	

    //2. Iterate over remaining segments (ordered by segment size) not yet assigned to an object - keep on expanding the search space.
    bool moreNodesAdded = true;
    std::map<int, std::vector<SolutionMapping *>> mappingsComputed;
    std::map<int, StructureGraph::Node *> * nodesInScene = m_sceneToSolve->m_structureGraph->m_nodes;   //Change this temporarily to check for specific nodes in scene. In general it should be all nodes.	

    std::vector<SolutionNode *> leaf_nodes;
    histories = new PointCloudSceneSolution::vg_histories();	

    /****** Seeded search .. for when we want to make sure the search proceeds nicely****/	


    if (m_sceneToSolve->m_pointCloud->GetPCSId() == 20001) 
    {
        std::cout << "Setting Seeds for 20001" << std::endl;
        biased = true;
        m_seeds[536]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[536].second.push_back(537); m_seeds[536].second.push_back(538); m_seeds[536].second.push_back(47);
		histories->AddSceneMapping(20001, 536, 10000, 0, 9);
        
        m_seeds[539]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[539].second.push_back(540); m_seeds[539].second.push_back(541); m_seeds[539].second.push_back(500);  m_seeds[539].second.push_back(177); m_seeds[539].second.push_back(172);
		histories->AddSceneMapping(20001, 539, 10000, 0, 9);

        m_seeds[542]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[542].second.push_back(543); m_seeds[542].second.push_back(544); m_seeds[542].second.push_back(421);
		histories->AddSceneMapping(20001, 542, 10000, 0, 4);

        m_seeds[546]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[546].second.push_back(545); m_seeds[546].second.push_back(547); m_seeds[546].second.push_back(226); m_seeds[546].second.push_back(443);
		histories->AddSceneMapping(20001, 546, 10000, 0, 9);

        m_seeds[549]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[549].second.push_back(548); m_seeds[549].second.push_back(550); m_seeds[549].second.push_back(243); m_seeds[549].second.push_back(233); m_seeds[549].second.push_back(463);
		histories->AddSceneMapping(20001, 549, 10000, 0, 9);

		m_seed_order.push_back(546); m_seed_order.push_back(549); m_seed_order.push_back(539); m_seed_order.push_back(536); m_seed_order.push_back(542);

		/* curvature = 10
		m_seeds[451]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[451].second.push_back(450); m_seeds[451].second.push_back(452); m_seeds[451].second.push_back(365); m_seeds[451].second.push_back(372);
		histories->AddSceneMapping(20001, 451, 10000, 0, 9);
        
        m_seeds[448]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[448].second.push_back(447); m_seeds[448].second.push_back(449); m_seeds[448].second.push_back(353);
		histories->AddSceneMapping(20001, 448, 10000, 0, 9);

        m_seeds[441]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[441].second.push_back(442); m_seeds[441].second.push_back(443); m_seeds[441].second.push_back(312); m_seeds[441].second.push_back(324); m_seeds[441].second.push_back(411);
		histories->AddSceneMapping(20001, 441, 10000, 0, 9);

        m_seeds[438]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[438].second.push_back(439); m_seeds[438].second.push_back(440); m_seeds[438].second.push_back(43); m_seeds[438].second.push_back(145);
		histories->AddSceneMapping(20001, 438, 10000, 0, 9);

        m_seeds[444]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[444].second.push_back(445); m_seeds[444].second.push_back(446); m_seeds[444].second.push_back(159); m_seeds[444].second.push_back(409);
		histories->AddSceneMapping(20001, 444, 10000, 0, 9);

		m_seed_order.push_back(451); m_seed_order.push_back(448); m_seed_order.push_back(438); m_seed_order.push_back(444); m_seed_order.push_back(441);
		*/
    }
                                                            
    if (m_sceneToSolve->m_pointCloud->GetPCSId() == 20002) 
    { 
        std::cout << "Setting Seeds for 20002" << std::endl; 	
        biased = true;
        m_seeds[4962]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[4962].second.push_back(5006); m_seeds[4962].second.push_back(4980); m_seeds[4962].second.push_back(5003); m_seeds[4962].second.push_back(4454);
		histories->AddSceneMapping(20002, 4962, 10003, 2, 9);        
        
		m_seeds[5020]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[5020].second.push_back(5018); m_seeds[5020].second.push_back(5019); m_seeds[5020].second.push_back(5021);
        histories->AddSceneMapping(20002, 5020, 10003, 0, 9);

		m_seeds[5014]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[5014].second.push_back(3223); m_seeds[5014].second.push_back(5015); m_seeds[5014].second.push_back(4992); m_seeds[5014].second.push_back(5017);		
		histories->AddSceneMapping(20002, 5014, 10003, 0, 9);        

        m_seeds[5030]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[5030].second.push_back(2800); m_seeds[5030].second.push_back(5031); m_seeds[5030].second.push_back(4689);
		histories->AddSceneMapping(20002, 5030, 10003, 0, 9);

        m_seeds[5035]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[5035].second.push_back(4835); m_seeds[5035].second.push_back(5035); m_seeds[5035].second.push_back(5036);
		histories->AddSceneMapping(20002, 5035, 10003, 0, 9);

        m_seeds[5010]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[5010].second.push_back(4298); m_seeds[5010].second.push_back(4988); m_seeds[5010].second.push_back(5011); m_seeds[5010].second.push_back(4533);
        histories->AddSceneMapping(20002, 5010, 10003, 0, 9);
		
        m_seeds[4955]=std::pair<bool,std::vector<int>>(true, std::vector<int>());  //table
        m_seeds[4955].second.push_back(310); m_seeds[4955].second.push_back(1975);
        histories->AddSceneMapping(20002, 4955, 11001, 2, 9);
		
        m_seeds[4076]=std::pair<bool,std::vector<int>>(true, std::vector<int>());    //box
        m_seeds[4076].second.push_back(4056); m_seeds[4076].second.push_back(4053); m_seeds[4076].second.push_back(4038); m_seeds[4076].second.push_back(4033);
		histories->AddSceneMapping(20002, 4076, 12000, 2, 9);

        //m_seeds[5025]=std::pair<bool,std::vector<int>>(true, std::vector<int>());    /*occluded sofa*/
        //m_seeds[5025].second.push_back(4998); m_seeds[5025].second.push_back(5001); m_seeds[5025].second.push_back(5026); m_seeds[5025].second.push_back(5027); m_seeds[5025].second.push_back(5028);
        m_seeds[4998]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[4998].second.push_back(5025); m_seeds[4998].second.push_back(5001); m_seeds[4998].second.push_back(5026); m_seeds[4998].second.push_back(5027); m_seeds[4998].second.push_back(5028);
		histories->AddSceneMapping(20002, 4998, 10003, 2, 9);

		m_seed_order.push_back(4076); m_seed_order.push_back(4962); m_seed_order.push_back(5020); m_seed_order.push_back(5014); 
		m_seed_order.push_back(5035); m_seed_order.push_back(5030); m_seed_order.push_back(5010); m_seed_order.push_back(4955);	 
		m_seed_order.push_back(4998);
    }

	if (m_sceneToSolve->m_pointCloud->GetPCSId() == 20003) 
    {
        std::cout << "Setting Seeds for 20003" << std::endl;
        biased = true;
        m_seeds[1263]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[1263].second.push_back(1953); m_seeds[1263].second.push_back(1872); m_seeds[1263].second.push_back(1956); m_seeds[1263].second.push_back(1317);
		m_seeds[1263].second.push_back(1345); m_seeds[1263].second.push_back(1957);	m_seeds[1263].second.push_back(1951); m_seeds[1263].second.push_back(1325);	
        histories->AddSceneMapping(20003, 1263, 10001, 1, 9);

        m_seeds[921]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[921].second.push_back(956);
		histories->AddSceneMapping(20003, 921, 11001, 0, 4);
		
        m_seeds[1572]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[1572].second.push_back(1659); m_seeds[1572].second.push_back(1703);
		histories->AddSceneMapping(20003, 1572, 12000, 1, 9);

        m_seeds[0]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[0].second.push_back(17); m_seeds[0].second.push_back(66); m_seeds[0].second.push_back(1894);
		histories->AddSceneMapping(20003, 0, 12000, 1, 9);
		
        m_seeds[388]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[388].second.push_back(314); 
		histories->AddSceneMapping(20003, 388, 12000, 1, 9);
		
		m_seed_order.push_back(388);		
		m_seed_order.push_back(1572);
		m_seed_order.push_back(0);
		m_seed_order.push_back(1263);
		m_seed_order.push_back(921); 				
    }

	if (m_sceneToSolve->m_pointCloud->GetPCSId() == 20022) 
    { 
		std::cout << "Setting Seeds for 20022" << std::endl;
		biased = true;
		m_seeds[970]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[970].second.push_back(967); m_seeds[970].second.push_back(968); m_seeds[970].second.push_back(969); m_seeds[970].second.push_back(816); m_seeds[970].second.push_back(817);
		m_seeds[970].second.push_back(818); m_seeds[970].second.push_back(875); m_seeds[970].second.push_back(876); m_seeds[970].second.push_back(971);
		histories->AddSceneMapping(20022, 970, 10003, 0, 9);

		m_seeds[961]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[961].second.push_back(958); m_seeds[961].second.push_back(678); m_seeds[961].second.push_back(959);
		histories->AddSceneMapping(20022, 961, 10003, 0, 9);

		m_seeds[354]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[354].second.push_back(361); m_seeds[354].second.push_back(854);
		histories->AddSceneMapping(20022, 354, 11001, 0, 9);

		m_seeds[931]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[931].second.push_back(930); m_seeds[931].second.push_back(932); m_seeds[931].second.push_back(629); m_seeds[931].second.push_back(609);
		histories->AddSceneMapping(20022, 931, 10000, 0, 9);
		
		m_seeds[950]=std::pair<bool,std::vector<int>>(true, std::vector<int>());
        m_seeds[950].second.push_back(951); m_seeds[950].second.push_back(949); m_seeds[950].second.push_back(697); m_seeds[950].second.push_back(695); m_seeds[950].second.push_back(696);
		m_seeds[950].second.push_back(698); m_seeds[950].second.push_back(699); m_seeds[950].second.push_back(700); m_seeds[950].second.push_back(701); m_seeds[950].second.push_back(702);
		histories->AddSceneMapping(20022, 950, 13000, 0, 9);
		
		m_seed_order.push_back(961); m_seed_order.push_back(970); m_seed_order.push_back(354); m_seed_order.push_back(931); m_seed_order.push_back(950);
	}

    /*******************/

    while (moreNodesAdded) {
        int beforeSearchNodesCount = m_markovDecisionProcess->size;
        int prevSegmentUsed = -1;
        
        //Gather current leaf nodes from search space
        for (auto n : m_markovDecisionProcess->m_nodes) if (n->IsLeaf()) leaf_nodes.push_back(n);
        std::cout << "\n\n" << std::endl;
        std::cout << "[Next Search Iteration] Number of Leaf Nodes == " << leaf_nodes.size() << std::endl;

        for (auto n : leaf_nodes)
        {
            StructureGraph::Node * nextGraphNode;
			auto structureGraphNode = nodesInScene->begin();
            bool firstSet = false; //Boolean set to true when the first node is selected. Further nodes are compared with this (in size)
            //for (auto structureGraphNode = nodesInScene->begin(); structureGraphNode != nodesInScene->end(); structureGraphNode++)
			for (int nxt_seed : m_seed_order)
            {
				structureGraphNode = nodesInScene->find(nxt_seed);
                //if (m_seeds.find(structureGraphNode->second->id) != m_seeds.end())			//Seeding Enabled (Debugging)
				std::cout << "Checking In Seeds for :: " << structureGraphNode->first << std::endl;
				if (m_seeds.find(nxt_seed) != m_seeds.end())			//Seeding Enabled (Debugging)
                {
                    //if (m_sceneToSolve->m_structureGraph->IsNodeMerged(structureGraphNode->second->id)) continue;  //Do not consider merged nodes/segments as anchors.
					if (m_sceneToSolve->m_structureGraph->IsNodeMerged(nxt_seed)) continue;  //Do not consider merged nodes/segments as anchors.
                    if (structureGraphNode->second->segment->points->size() > 100)
                    {
                        if (n->SegmentUsed(structureGraphNode->first)) continue;
                        if (!firstSet) { nextGraphNode = structureGraphNode->second; firstSet=true; continue; }
                        //if (structureGraphNode->second->stats->point_count > nextGraphNode->stats->point_count) nextGraphNode = structureGraphNode->second;
                    }
                }
            }
            //std::cout << "firstset == " << firstSet << std::endl;
            //Next Segment to fit selected - {Need to establish what objects we're searching for here - depending on scene - or current context}
            //Make sure the segment selected has not been used already. This should happen only when first set is true
            if (firstSet) 
            {
                std::cout << "Crt Path:: "; m_markovDecisionProcess->PrintLeafFirstTraversal(n->id);
                MatchSceneSegmentWithObjectClasses(n, nextGraphNode, mappingsComputed); //A reference to the already computed mappings is passed as argument.
            }
        }

        leaf_nodes.clear();
        //std::cout << "OriginalSize=" << beforeSearchNodesCount << " CurrentSize=" << m_markovDecisionProcess->size << std::endl;
        if (m_markovDecisionProcess->size == beforeSearchNodesCount) moreNodesAdded = false;
    }
    
    m_markovDecisionProcess->PrintLeafFirstTraversal(); //Print out solutions

    return m_markovDecisionProcess->size;
}

//1. Search for boundaries;
//2. Search for objects (chairs, tables) using the trained models DB and establish relationships
int PointCloudSceneSolution::ComputeMDPSearchSpace2()
{
    int searchSpaceSize = 0;
    m_sceneToSolve->ComputeOBB(0.0f); //Compute OBB for Scene To Solve

    StructureGraph * structureGraph = m_sceneToSolve->m_structureGraph;
    SegmentsGraph * segmentsGraph = m_sceneToSolve->m_segmentsGraph;

    bool floor_present = false;					// Solution tag 'floor'
    bool walls_present = false;					// Solution tag 'wall'
    bool ceiling_present = false;				// Solution tag 'ceiling'

    new std::map<std::string, std::vector<std::pair<float, std::vector<int>>>>();
    
    //0. Promote internal nodes of compound segmetns to level0 from level1(internal)
    m_sceneToSolve->m_structureGraph->PromoteInternalNodesToLevel0();	

    //0. Shrink the search space first by automerging adjacent coplanar segments.
    m_sceneToSolve->MergeStates(false, 0, 2, 0.95f, 0.05f, 0);

    m_sceneToSolve->m_segmentsGraph->m_pcs->DrawSceneGraph(m_sceneToSolve->m_structureGraph, m_sceneToSolve->m_pointCloud->GetName(), 1.2f);

    //1. FitBoundaries
    SearchBoundaries(m_markovDecisionProcess->m_root, 0.95f, 0.2f, 2.2f);
    std::cout << "Nodes attached to root = " << m_markovDecisionProcess->m_root->GetNumberOfChildren() << std::endl;

    //Output Paths
    m_markovDecisionProcess->PrintLeafFirstTraversal();

    //2. Iterate over structures to fit - Enumerate the search space.


    return searchSpaceSize;
}


//Is this a chair, table, aeroplane, monitor, etc ...	
int PointCloudSceneSolution::MatchSceneSegmentWithObjectClasses(SolutionNode * _parentSolution, StructureGraph::Node * _segment, std::map<int, std::vector<SolutionMapping *>> &_mappings)
{	
    //To start off we're going to try fitting this segment in a chair or a table - start with chairs.
    std::cout << ">>MATCHING SEGMENT ID: " << _segment->id << " at Node ID: " << _parentSolution->id << std::endl;
    int newNodesAdded = 0;
    auto oldMappingIt = _mappings.find(_segment->id);
    bool can_reuse = true;
    std::vector<std::pair<PointCloudSegmentation *, std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats *>>> solutions;     //pair (trainingmodel, (vector of segments fitted, vg_stats))

    float bestScore = 0;

    if (oldMappingIt != _mappings.end())
    {
        //Some other solution node has already computed a solution mapping starting from this segment. Check if it can be fitted in our solution path.
        std::vector<SolutionMapping *> oldMappings = oldMappingIt->second;
        for (auto mappings_it = oldMappings.begin(); mappings_it != oldMappings.end(); mappings_it++)
        {
            can_reuse = true;
            for (auto pathSegmentsIt = (*mappings_it)->segments_use.begin(); pathSegmentsIt!= (*mappings_it)->segments_use.end(); pathSegmentsIt++)
            {
                if (_parentSolution->SegmentUsed(pathSegmentsIt->first)) { can_reuse = false; break; }
            }

            //If it can be used then attach to node - Eventually order the mappings that can be used and choose the highest scoring one - for now attach them all.
            /*if (can_reuse)
            {
                //std::cout << "Reusing previous mapping -- " << (*mappings_it)-> << std::endl;
                SolutionNode * new_node = m_markovDecisionProcess->CreateNode(_parentSolution);
                std::pair<float, std::vector<StructureGraph::Node *>> map_nodes;
                map_nodes.second.push_back(_segment);
                new_node->AddObjectMatchInMapping("dummy", map_nodes);
                _mappings[_segment->id].push_back(new_node->m_mapping);				
            }*/
        }
    } else { can_reuse = false; }
    
    if (!can_reuse)
    {
        //Computing new solution map starting from this segment - iterate over all object PCS in Model Training instance and determine if there are any (0 or more) which match
        //Note that we will end up using many more segments than the one we're starting from. But that makes perfect sense. We're simply choosing the root.
        //std::cout << "Computing new mapping --" << std::endl;
        m_trainedModelDB->ResetDBGlobalIndex();	//Reset index in db.
        m_trainedModelDB->ResetDBLocalIndex();  //Reset in class object index
        PointCloudSegmentation * db_model = m_trainedModelDB->GetNext();

        history = new PointCloudSceneSolution::vg_history();
		int best_model_id = 0;
		int best_anchor_score = 10000;
        while (db_model != NULL)
        {				
			std::pair<bool, SceneMapping*> m = histories->GetSceneMapping(m_sceneToSolve->m_pointCloud->GetPCSId(), _segment->id, db_model->m_id);
			if (m.first == false) { db_model = m_trainedModelDB->GetNext(); continue; }

            std::cout << "+++\n[SWITCHING MODELS] -> Now using " << db_model->m_id << "\n+++" <<std::endl;
            
            std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*> stats = MatchSceneSegmentWithTrainedObjectVoxelGrids(_parentSolution, db_model, _segment);
            if (stats.first.size() > 0)
            {
                if (stats.second->max_coverage > 0.0001f) //is there enough evidence to include in solutions vector for later checking. Another Parameter.
                {
                    solutions.push_back(std::pair<PointCloudSegmentation*, std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*>>(db_model, stats));
					int crt_anchor_score = stats.second->tgrid->ComputeAnchorDistanceWithGridUVW(stats.second->vgrid);
					std::cout << "Anchor Segment Distance:: " << crt_anchor_score << std::endl;
					if (crt_anchor_score < best_anchor_score) { best_anchor_score = crt_anchor_score; best_model_id = db_model->m_id; }
                }
            }
            db_model = m_trainedModelDB->GetNext();
        }
		
		std::cout << "DELETING MODELS FROM HISTORY EXCEPT FOR " << best_model_id << std::endl;
		history->DeleteHistoryUnitsExceptForModelId(best_model_id);

        histories->AddHistory(history);

        //Sort Compatibility score and choose best 1 or 2 (this will be a parameter later on - TODO) - TEMP:::: ONLY ONE FOR NOW - i.e. no branching in the enumeration of the search space
        if (solutions.size() > 0)
        {
            std::pair<PointCloudSegmentation*, std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*>> best_overall_solution = solutions.front();
            SolutionNode * new_node = m_markovDecisionProcess->CreateNode(_parentSolution);
            std::pair<float, std::vector<StructureGraph::Node *>> map_nodes;
            for (auto bs : best_overall_solution.second.first) 
			{
				if (m_sceneToSolve->m_structureGraph->m_nodes->find(bs) != m_sceneToSolve->m_structureGraph->m_nodes->end())
					map_nodes.second.push_back(m_sceneToSolve->m_structureGraph->m_nodes->at(bs));
			}
            new_node->AddObjectMatchInMapping(best_overall_solution.first->m_pointCloud->GetName(), map_nodes, best_overall_solution.second.second);
            _mappings[_segment->id].push_back(new_node->m_mapping);
        }
    }

    return newNodesAdded;
}

std::vector<int> PointCloudSceneSolution::GetSceneNodesConsistentWithFDGStructureString(FeatureDescStructureGraph * _fdg, StructureGraph::Node * _initialNode)
{
    std::vector<int> segs;


    return segs;
}

//Measure the distance between the segment (+neighbours) and the trained object
std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*> PointCloudSceneSolution::MatchSceneSegmentWithTrainedObjectVoxelGrids(SolutionNode * _parentSolution, PointCloudSegmentation * _object, StructureGraph::Node * _segment)
{	
    std::cout << "++Matching VGrids in FDSG " << _object->m_pointCloud->GetName() << " for segment " << _segment->id << std::endl;
    float best_overall_coverage = -100.0f;
    int best_model_grid_id = 0;
    int rotationCount = 36;
    float rot_diff = Maths::PiTwo / rotationCount;
    double start = Platform::GetTime();
    std::pair<float, std::pair<float, float>> voxel_grid_measure;
    bool more_trials = true;	
	int best_anchor_distance = 10000;
	int best_anchor_fdg_idx = -1;

    std::vector<std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*>> best_fits_per_anchor_segment;   //Currently number of anchors is set to 3
	int model_id = _object->m_id;	
	std::cout << "MODEL ID " << model_id << std::endl;	

    for (int i=0; i<_object->m_featureDescriptionGraphs->size(); i++)
    {					
		std::pair<bool, SceneMapping*> m = histories->GetSceneMapping(m_sceneToSolve->m_pointCloud->GetPCSId(), _segment->id, model_id);
		if (m.first!=false) if (m.second->fdg != i) continue;

        FeatureDescStructureGraph * fdg = _object->m_featureDescriptionGraphs->at(i);
		int fdg_id = fdg->initial_node_id;
        
        float t_step = 0.6f;
        float max_coverage = 0;
        float coverage = 0;

        int	total_vg_built = 0;

        int min_diff = 1000000;
        int max_pos = 0;
        int max_neg = 0;
        int best_grid = -1;
        Vector3 best_min_uvw_t;
        Vector3 best_max_uvw_t;
        float best_rot;
        int crt_best_grid = 0;
        std::vector<int> crt_best_segs;
        float crt_fdg_best_coverage = -100.0f;
        
		float radius = 12.0f;
        std::map<int,bool> visited_segs;
                
        std::vector<FeatureDescStructureGraph::VG_stats *> model_grids;
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_v_w, 0));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_v_inv_w, 1));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_v_w_inv, 2));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_v_inv_w_inv, 3));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_inv_v_w, 4));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_inv_v_inv_w, 5));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_inv_v_w_inv, 6));
        model_grids.push_back(new FeatureDescStructureGraph::VG_stats(fdg->m_grid_u_inv_v_inv_w_inv, 7));

        int average_grid_size = (fdg->m_grid_u_v_w->m_voxels->size() + 
                                 fdg->m_grid_u_v_inv_w->m_voxels->size() +
                                 fdg->m_grid_u_v_w_inv->m_voxels->size() +
                                 fdg->m_grid_u_v_inv_w_inv->m_voxels->size() +
                                 fdg->m_grid_u_inv_v_w->m_voxels->size() +
                                 fdg->m_grid_u_inv_v_inv_w->m_voxels->size() +
                                 fdg->m_grid_u_v_w_inv->m_voxels->size() +
                                 fdg->m_grid_u_inv_v_inv_w_inv->m_voxels->size()) / 8;

        auto fdg_structure_string = fdg->GetStringDescriptorMaxCoverageCrossPointCount(3);
                
        FeatureDescStructureGraph::VoxelGrid * crt_model_grid;
        FeatureDescStructureGraph::VoxelGrid * voxel_grid = new FeatureDescStructureGraph::VoxelGrid(1, 1, 1);
                
        std::vector<int> segs; segs.push_back(_segment->segment->id); visited_segs[_segment->segment->id] = true;
        int trials_count = 0;

        while (more_trials)
        {			
            trials_count++;
            if (trials_count > 24) { more_trials=false; std::cout << "BREAKING FROM MORE TRIALS" << std::endl; break; }
            //std::cout << "Fitting against Model " << _object->m_pointCloud->GetName() << ", DescID " << fdg->initial_node_id << " to " << segs.size() << " segments [";
            std::cout << "M:" << _object->m_pointCloud->GetName() << " FD:" << fdg->initial_node_id << "[";
            for (auto s : segs) std::cout << s << " ";
            std::cout << "]" << std::endl;
            //<< std::endl;
            int total_points_in_segs = 0;
            int total_points_in_downsampled_segs = 0;
            
			for (auto s : segs) 
            {
                total_points_in_segs += m_sceneToSolve->m_structureGraph->m_nodes->at(s)->segment->points->size();
                if (m_downsampledSegments->find(s) == m_downsampledSegments->end())
                {
                    boost::timer downsampling_timer;
                    //Create a downsampled version of this segment
                    SegmentsGraph::SurfaceSegment * ss = m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(s);
                    (*m_downsampledSegments)[s] = new std::vector<int>();
                    auto pointListIterator = m_downsampledSegments->find(s);
                    std::vector<Vector3> seed_points;
                    bool addToSeeds = true;
                    Vector3 crtPoint;
                    Vector3 crtSeed;
                    for (int i=0; i<ss->points->size(); i++)
                    {
                        crtPoint = m_sceneToSolve->m_pointCloud->m_pointList.at(ss->points->at(i)).m_vertex.Position;
                        for (int j=0; j<seed_points.size(); j++)
                        {
                            crtSeed = seed_points.at(j);
                            if (Vector3::DistanceSquared(crtSeed, crtPoint) < 1.0f) { addToSeeds = false; break; }
                        }
                        if (addToSeeds) { seed_points.push_back(crtPoint); pointListIterator->second->push_back(ss->points->at(i)); m_sceneToSolve->m_pointCloud->m_pointList.at(ss->points->at(i)).m_seed = true; }
                        addToSeeds = true;
                    }
                    //std::cout << ">>Down-sampling for segment " << s << " finished in " << downsampling_timer.elapsed() << " " << ss->points->size() << "->" << seed_points.size() << std::endl;
                }

                total_points_in_downsampled_segs += m_downsampledSegments->find(s)->second->size();
            }
            
            #pragma omp parallel for
            for (int vg_idx = 0; vg_idx<model_grids.size(); vg_idx++)
			{
                //if (vg_idx==0) continue;
                //if (vg_idx==1) continue;          /*sofa big*/
                //if (vg_idx==2) continue;
                //if (vg_idx==3) continue;
                //if (vg_idx==4) continue;
                //if (vg_idx==5) continue;
                //if (vg_idx==6) continue;
                //if (vg_idx==7) continue;				

				if (m.first == true)
				{
					if (m.second->id == m_sceneToSolve->m_pointCloud->GetPCSId())
					{
						if (m.second->segment == _segment->id)
						{							
							if (m.second->grid < 8 && m.second->grid != vg_idx) 
							{
								//std::cout << vg_idx << ":" << m.second->id << "," << m.second->segment << " BREAK" << std::endl;
								continue;
							}
							else
							{
								//std::cout << vg_idx << ":" << m.second->id << "," << m.second->segment << " Moving ON" << std::endl;
							}
						}
					}
				}
	            
				FeatureDescStructureGraph::VG_stats * crt_stats = model_grids.at(vg_idx);
                FeatureDescStructureGraph::VoxelGrid * crt_model_grid = crt_stats->tgrid;
                Vector3 model_u(crt_model_grid->uvw.U.X, crt_model_grid->uvw.U.Y, crt_model_grid->uvw.U.Z);
                Vector3 model_v(crt_model_grid->uvw.V.X, crt_model_grid->uvw.V.Y, crt_model_grid->uvw.V.Z);
                Vector3 model_w(crt_model_grid->uvw.W.X, crt_model_grid->uvw.W.Y, crt_model_grid->uvw.W.Z);                
                bool all_points_in_grid = false;
                float inclusion_bias = 0.f;
				float segment_inclusion_bias = 1.0f;
                float anchor_match_bias = 0.f;
                float non_anchor_match_bias = 0.f;                				

                int voxels_in_model = crt_model_grid->m_voxels->size();
                Vector3 min_t(0); Vector3 max_t(0);
                FeatureDescStructureGraph::VoxelGrid::Match_Result result;
                for (int j = 0; j<rotationCount; j++)
                {
                    all_points_in_grid = false;
                    min_t.X = 0; min_t.Y = 0; min_t.Z = 0;
                    max_t.X = 0; max_t.Y = 0; max_t.Z = 0;
                    int iterations = 0;
                    while (!all_points_in_grid)
                    {
                        iterations++; if (iterations > 20) { all_points_in_grid=true; crt_stats->coverage = 0.0001f; /*std::cout << "AHHHHH Quiting all_points_in_grid loop!!! VG::" << vg_idx << std::endl; */break; }
                        m_sceneToSolve->ComputeVoxelGridAroundDownsampledSegment(segs, crt_model_grid, 1, rot_diff*j, min_t, max_t, crt_stats->vgrid, m_downsampledSegments);
                        crt_stats->total_vg_built++;
						
						/*int crt_anchor_score = 1000000;
						int points_out = crt_stats->vgrid->above_u_max + crt_stats->vgrid->below_u_min +
                                        crt_stats->vgrid->below_v_min + crt_stats->vgrid->above_v_max +
                                        crt_stats->vgrid->below_w_min + crt_stats->vgrid->above_w_max;
						if (points_out == 0)
						{
							crt_anchor_score = crt_model_grid->ComputeAnchorDistanceWithGridUVW(crt_stats->vgrid);
						}
						*/

                        crt_model_grid->ComputeOverlapPosWithGridUVW(crt_stats->vgrid, total_points_in_downsampled_segs, segs.size(), result);
						crt_stats->coverage = result.coverage;
                        if (crt_stats->coverage >= crt_stats->max_coverage)
                        {
							//std::cout << "Updating Coverage (" << vg_idx << ") NewCells:" << result.new_cells << " Cov:" << crt_stats->max_coverage << " > " << crt_stats->coverage << " InAnchorMatches:" << result.anchor_matches << "(" << result.anchor_match_bias << ") --- NotInAnchorMatches:" << result.not_in_anchor_matches <<  "(" << result.non_anchor_match_bias << ")" << " Rot:" << rot_diff*i << " IB:" << result.inclusion_bias << std::endl;
							crt_stats->max_coverage = result.coverage;
                            crt_stats->max_inclusion_bias = result.inclusion_bias;
                            crt_stats->max_non_anchor_matches = result.not_in_anchor_matches;
                            crt_stats->max_anchor_matches = result.anchor_matches;
							crt_stats->max_anchor_match_bias = result.anchor_match_bias;
							crt_stats->max_non_anchor_match_bias = result.non_anchor_match_bias;
                            crt_stats->best_rot = rot_diff*j;
							crt_stats->new_cells = result.new_cells;
                            crt_stats->best_min_uvw_t.X = min_t.X; crt_stats->best_min_uvw_t.Y = min_t.Y; crt_stats->best_min_uvw_t.Z = min_t.Z;
                            crt_stats->best_max_uvw_t.X = max_t.X; crt_stats->best_max_uvw_t.Y = max_t.Y; crt_stats->best_max_uvw_t.Z = max_t.Z;
                            crt_stats->segs.clear();
                            for (auto segs_it : segs) crt_stats->segs.push_back(segs_it);

							//std::cout << "VGrid Size::" << crt_stats->vgrid->m_voxels->size() << " InOriginCount:before::" << crt_stats->vgrid->GetVoxelCountInOrigin() << std::endl;
							
							//std::vector<int> neighbourEdgeSegments = m_sceneToSolve->m_segmentsGraph->GetConnectedEdgeSegments(segs);
							//crt_stats->vgrid->u_cells_count * (crt_stats->vgrid->grid_cell_spacing_u /2);							
							/*
							std::vector<int> neighbourEdgeSegments = m_sceneToSolve->m_segmentsGraph->GetEdgeSegmentsWithinSphere(crt_stats->vgrid->o_model_xyz, radius);							
							//std::cout << "Centre::" << crt_stats->vgrid->o_model_xyz.ToString() << " Radius::" << radius << " Edge Segments Considered :: " << neighbourEdgeSegments.size() << std::endl;
							std::vector<int> useful_edges;
							for (auto n_ids : neighbourEdgeSegments) 
							{
								std::vector<int> crt_edges; for (auto useful : useful_edges) crt_edges.push_back(useful);
								crt_edges.push_back(n_ids);
								FeatureDescStructureGraph::VoxelGrid tmp_voxel_grid(1,1,1);
								m_sceneToSolve->ComputeVoxelGridAroundDownsampledSegment(segs, crt_model_grid, 1, rot_diff*j, min_t, max_t, &tmp_voxel_grid, m_downsampledSegments);
								m_sceneToSolve->AugmentVoxelGridWithEdgeSegments(&tmp_voxel_grid, crt_edges);
								if (tmp_voxel_grid.m_voxels->size() == crt_stats->vgrid->m_voxels->size()) continue; //no increase in voxel size == no improvements. Useless edge
								FeatureDescStructureGraph::VoxelGrid::Match_Result tmp_result;
								crt_model_grid->ComputeOverlapPosWithGridUVW(&tmp_voxel_grid, total_points_in_downsampled_segs, segs.size(), tmp_result);								
								if (tmp_result.coverage > crt_stats->coverage)
								{
									//std::cout << "+++ Increasing Coverage from " << crt_stats->max_coverage << " to " << tmp_result.coverage << " when using edge ids ";
									//for (auto crt_edges_ids : crt_edges) std::cout << " " << crt_edges_ids; std::cout << ". ";
									//std::cout << "+++ NewCells " << result.new_cells << " > " << tmp_result.new_cells << std::endl;
									//Update score
									//crt_stats->max_coverage = tmp_result.coverage;
									crt_stats->coverage = tmp_result.coverage;									
									crt_stats->max_inclusion_bias = tmp_result.inclusion_bias;
									crt_stats->max_non_anchor_matches = tmp_result.not_in_anchor_matches;
									crt_stats->max_anchor_matches = tmp_result.anchor_matches;
									crt_stats->max_anchor_match_bias = tmp_result.anchor_match_bias;
									crt_stats->max_non_anchor_match_bias = tmp_result.non_anchor_match_bias;									
									crt_stats->new_cells = tmp_result.new_cells;
									crt_stats->vgrid->CopyFrom(tmp_voxel_grid);									
									crt_stats->segs.push_back(n_ids);
									useful_edges.push_back(n_ids);

									result.coverage = tmp_result.coverage;
									result.anchor_match_bias = tmp_result.anchor_match_bias;
									result.non_anchor_match_bias = tmp_result.non_anchor_match_bias;
									result.anchor_matches = tmp_result.anchor_matches;
									result.not_in_anchor_matches = tmp_result.not_in_anchor_matches;
									result.new_cells = tmp_result.new_cells;
								}
							}*/							
                        }

                        if (vg_idx==0)
							history->m_vg_history_0->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==1)
                            history->m_vg_history_1->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==2)
                            history->m_vg_history_2->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==3)
                            history->m_vg_history_3->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==4)
                            history->m_vg_history_4->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==5)
                            history->m_vg_history_5->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==6)
                            history->m_vg_history_6->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        if (vg_idx==7)
                            history->m_vg_history_7->push_back( vg_history::history_unit(segs, new FeatureDescStructureGraph::VoxelGrid(*(crt_stats->vgrid)), crt_model_grid->o_model_xyz, result.coverage, 
							result.inclusion_bias, result.anchor_match_bias, result.non_anchor_match_bias, result.anchor_matches, result.not_in_anchor_matches, model_id, vg_idx, model_u, model_v, model_w, fdg_id));

                        all_points_in_grid=true;
                        
                        if (crt_stats->vgrid->above_u_max > 0 | crt_stats->vgrid->below_u_min > 0) { min_t.X += t_step; max_t.X += t_step; all_points_in_grid = false; } //std::cout << " U++ Above::" << crt_stats->vgrid->above_u_max << " Dst:" << crt_stats->vgrid->dist_above_u_max << " Below::" << voxel_grid->below_u_min << " Dst:" << voxel_grid->above_u_max << std::endl; }
                        if (crt_stats->vgrid->below_v_min > 0) { min_t.Y += t_step; all_points_in_grid=false; } //std::cout << " BelowV::" << crt_stats->vgrid->below_v_min << " Dst:" << crt_stats->vgrid->dist_below_v_min << std::endl; }
                        if (crt_stats->vgrid->above_v_max > 0) { max_t.Y += t_step; all_points_in_grid=false; } //std::cout << " AboveV::" << crt_stats->vgrid->above_v_max << " Dst:" << crt_stats->vgrid->dist_above_v_max << std::endl; }
                        if (crt_stats->vgrid->below_w_min > 0) { min_t.Z += t_step; all_points_in_grid=false; } //std::cout << " BelowW::" << crt_stats->vgrid->below_w_min << " Dst:" << crt_stats->vgrid->dist_below_w_min << std::endl; }
                        if (crt_stats->vgrid->above_w_max > 0) { max_t.Z += t_step; all_points_in_grid=false; } //std::cout << " AboveW::" << crt_stats->vgrid->above_w_max << " Dst:" << crt_stats->vgrid->dist_above_w_max << std::endl; }                        
                    }	
                    min_t.X = 0; min_t.Y = 0; min_t.Z = 0;
                    max_t.X = 0; max_t.Y = 0; max_t.Z = 0;
                }
            }

            max_coverage = -100;
			int max_m = 0;
            for (int i=0; i<model_grids.size(); i++)
            {
                //std::cout << "Grid:" << i << " MaxCov::" << model_grids.at(i)->max_coverage << " Model VG Size::" <<  model_grids.at(i)->tgrid->m_voxels->size() << std::endl;
				
				std::cout << "*G" << i << "#" << model_grids.at(i)->tgrid->m_voxels->size() << " [C:" << model_grids.at(i)->max_coverage << "], NonAnchorM:" << 
				model_grids.at(i)->max_non_anchor_matches << " (" << model_grids.at(i)->max_non_anchor_match_bias << "), AnchorM:" << model_grids.at(i)->max_anchor_matches << 
				" (" << model_grids.at(i)->max_anchor_match_bias << "), IB:" << model_grids.at(i)->max_inclusion_bias << " VoxelsInAnchorScene:" << model_grids.at(i)->vgrid->GetVoxelCountInOrigin() 
				<< " VoxelsInAnchorModel:" << model_grids.at(i)->tgrid->GetVoxelCountInOrigin() << " NewC:" << model_grids.at(i)->new_cells << std::endl;				
											
                if (model_grids.at(i)->max_coverage > max_coverage) { max_coverage = model_grids.at(i)->max_coverage; best_grid = i; }
                total_vg_built += model_grids.at(i)->total_vg_built;
            }

            if (max_coverage == -100) { best_grid = 0; std::cout << "MAX COVERAGE PROBLEM ---- ZERO" << std::endl; }
            std::cout << "+G" << best_grid << " [C:" << model_grids.at(best_grid)->max_coverage << ", MaxNonAnchorMatches:" << model_grids.at(best_grid)->max_non_anchor_matches << ", MaxAnchorMatches:" << model_grids.at(best_grid)->max_anchor_matches << ", IB:" << model_grids.at(best_grid)->max_inclusion_bias << "]\n\n";
                        
            crt_model_grid = model_grids.at(best_grid)->tgrid;
            history->CopyOptimalGridLineToBestLine(best_grid);
            more_trials = false;                       
            
			if ((model_grids.at(best_grid)->max_coverage) >= (crt_fdg_best_coverage))
            {
                //store crt best
                crt_best_grid = best_grid;
                best_min_uvw_t = model_grids.at(best_grid)->best_min_uvw_t;
                best_max_uvw_t = model_grids.at(best_grid)->best_max_uvw_t;
                best_rot = model_grids.at(best_grid)->best_rot;
                crt_fdg_best_coverage = model_grids.at(best_grid)->max_coverage;
                if (crt_fdg_best_coverage > best_overall_coverage) best_overall_coverage = crt_fdg_best_coverage;
        
                crt_best_segs.clear(); for (auto s : model_grids.at(best_grid)->segs) crt_best_segs.push_back(s);

                //std::cout << "Updating best_overall_coverage to :: " << best_overall_coverage << std::endl;
                
                //See if another segment can be added to segs					
                for (auto segs_present : segs)
                {
                    if (more_trials) break;
                    m_sceneToSolve->m_structureGraph->m_nodes->at(segs_present);
                    std::pair<int,int> nxtBestSeg(0,0);

                    if (biased)
                    {                        
						for (auto s_neighbours : m_seeds[_segment->segment->id].second)
                        {
                            if (_parentSolution->SegmentUsed(s_neighbours)) continue;
                            if (visited_segs.find(s_neighbours) != visited_segs.end()) continue;
							if (m_sceneToSolve->m_structureGraph->m_nodes->find(s_neighbours) == m_sceneToSolve->m_structureGraph->m_nodes->end()) continue;
                            if (m_sceneToSolve->m_structureGraph->m_mergedNodesMap->find(s_neighbours) != m_sceneToSolve->m_structureGraph->m_mergedNodesMap->end()) continue;								
                            nxtBestSeg.first = s_neighbours; nxtBestSeg.second = 1; break;
                        }
                    }						
                    else
                    {
                        /*for (auto s_neighbours : (*m_sceneToSolve->m_structureGraph->m_nodes->at(segs_present)->edges))
                        {
                            if (_parentSolution->SegmentUsed(s_neighbours.first)) continue;
                            if (visited_segs.find(s_neighbours.first) != visited_segs.end()) continue;
                            if (m_sceneToSolve->m_structureGraph->m_mergedNodesMap->find(s_neighbours.first) != m_sceneToSolve->m_structureGraph->m_mergedNodesMap->end()) continue;
                            int pntCnt = s_neighbours.second->second->segment->points->size();
                            if (pntCnt > nxtBestSeg.second) { nxtBestSeg.first = s_neighbours.first; nxtBestSeg.second = pntCnt; }
                        }*/
                    }

                    //Ideally check for compatibility with FDG structure string here. For now just add it
                    if (nxtBestSeg.first != 0)
                    {
                        segs.push_back(nxtBestSeg.first);
                        visited_segs[nxtBestSeg.first] = true;
                        more_trials = true;
                        break;
                    }
                }					
                
            }
            else
            {
                //Revert to the previous best_grid. Try some other segment? if not possible set more_trials to false;
                //if (!biased)
                //{
                /*    segs.clear(); 
                    for (auto s : crt_best_segs) segs.push_back(s);				
                    for (auto segs_present : segs)
                    {
                        if (more_trials) break;
                        m_sceneToSolve->m_structureGraph->m_nodes->at(segs_present);
                        for (auto s_neighbours : (*m_sceneToSolve->m_structureGraph->m_nodes->at(segs_present)->edges))
                        {
                            if (_parentSolution->SegmentUsed(s_neighbours.first)) continue;
                            if (visited_segs.find(s_neighbours.first) != visited_segs.end()) continue;
                            //Ideally check for compatibility with FDG structure string here. For now just add it
                            segs.push_back(s_neighbours.first);
                            visited_segs[s_neighbours.first] = true;
                            more_trials = true;
                            break;
                        }
                    }*/
                //}
            }			
        }

        //Recompute VG around best segs using best_grid parameters and augment it with edge point data. Recompute score ...
		int total_points_in_downsampled_segs = 0;
		for (auto s : model_grids.at(crt_best_grid)->segs) 
			if (m_downsampledSegments->find(s) != m_downsampledSegments->end()) total_points_in_downsampled_segs += m_downsampledSegments->find(s)->second->size();
        crt_model_grid = model_grids.at(crt_best_grid)->tgrid;
		FeatureDescStructureGraph::VoxelGrid * final_voxel_grid = new FeatureDescStructureGraph::VoxelGrid(1,1,1);
		m_sceneToSolve->ComputeVoxelGridAroundDownsampledSegment(model_grids.at(crt_best_grid)->segs, crt_model_grid, 1, model_grids.at(crt_best_grid)->best_rot, best_min_uvw_t, best_max_uvw_t, final_voxel_grid, m_downsampledSegments);
		
		//std::vector<int> neighbourEdgeSegments = m_sceneToSolve->m_segmentsGraph->GetConnectedEdgeSegments(segs);
		//crt_stats->vgrid->u_cells_count * (crt_stats->vgrid->grid_cell_spacing_u /2);							
		std::vector<int> neighbourEdgeSegments = m_sceneToSolve->m_segmentsGraph->GetEdgeSegmentsWithinSphere(model_grids.at(crt_best_grid)->vgrid->o_model_xyz, radius);
							
		//std::cout << "Edge Segments Considered :: " << neighbourEdgeSegments.size() << " [";
		//for (auto n_ids : neighbourEdgeSegments) std::cout << n_ids << " "; std::cout << "]" << std::endl; 
		FeatureDescStructureGraph::VoxelGrid tmp_voxel_grid(*final_voxel_grid);
		m_sceneToSolve->AugmentVoxelGridWithEdgeSegments(&tmp_voxel_grid, neighbourEdgeSegments);				        
        FeatureDescStructureGraph::VoxelGrid::Match_Result result_1;
		FeatureDescStructureGraph::VoxelGrid::Match_Result result_2;
        crt_model_grid->ComputeOverlapPosWithGridUVW(final_voxel_grid, total_points_in_downsampled_segs, model_grids.at(crt_best_grid)->segs.size(), result_1);
		crt_model_grid->ComputeOverlapPosWithGridUVW(&tmp_voxel_grid, total_points_in_downsampled_segs, model_grids.at(crt_best_grid)->segs.size(), result_2);
               
		std::cout << "FDG RESULT--> COVERAGE Score:: " << result_1.coverage << " A_COVERAGE Score:: " << result_2.coverage << std::endl;

        //voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);
        FeatureDescStructureGraph::VG_stats * best_for_anchor = new FeatureDescStructureGraph::VG_stats(crt_model_grid, crt_best_grid);
		best_for_anchor->best_max_uvw_t = model_grids.at(crt_best_grid)->best_max_uvw_t;
        best_for_anchor->best_min_uvw_t = model_grids.at(crt_best_grid)->best_min_uvw_t;
        best_for_anchor->best_rot = model_grids.at(crt_best_grid)->best_rot;
		for (auto s : model_grids.at(crt_best_grid)->segs) best_for_anchor->segs.push_back(s);		
		best_for_anchor->max_coverage = result_1.coverage;
		best_for_anchor->coverage = result_1.coverage;
		if (result_2.coverage > result_1.coverage) 
		{
			best_for_anchor->max_coverage = result_2.coverage;
			best_for_anchor->coverage = result_2.coverage;		
		}
        best_for_anchor->vgrid = final_voxel_grid;
		best_for_anchor->new_cells = result_2.new_cells;
		best_for_anchor->max_anchor_matches = result_2.anchor_matches;
		best_for_anchor->max_non_anchor_matches = result_2.not_in_anchor_matches;
		best_for_anchor->max_pos = result_2.segments_used.size();
		best_for_anchor->min_diff = model_grids.at(crt_best_grid)->min_anchor_diff;
		best_for_anchor->grid_idx = fdg->initial_node_id;   //Temp Fix
                    
        best_fits_per_anchor_segment.push_back(std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*>(crt_best_segs, best_for_anchor));
		
        more_trials = true;
        std::cout << "--> Finished in " << Platform::ToSeconds(Platform::GetTime()-start) << ". Moving on to next FDG." << std::endl;        
    }
    
    std::cout << "Time Elapsed::" << Platform::ToSeconds(Platform::GetTime()-start) << " ... AND THE RESULTS ARE ****** " << std::endl;
    
    int best_fit = 0; float best_score = 0.f; int highest_match_count = 0; int idx = 0;
	int smallest_anchor_diff = 100000; int best_anchor_diff_idx = 0; float best_score_a = 0.f;
	std::vector<int> anchor_diffs;
    for (auto best_fits : best_fits_per_anchor_segment)
    {
		int anchor_score = best_fits.second->tgrid->ComputeAnchorDistanceWithGridUVW(best_fits.second->vgrid);
		anchor_diffs.push_back(anchor_score);
		float segment_bias = 1 + (0.1f*best_fits.second->max_pos);
		int matches = best_fits.second->max_non_anchor_matches * segment_bias;
        float crt_score = best_fits.second->coverage;      
		std::cout << "==> FDG_Idx::" << idx << ")" << crt_score << " NewCells:" << best_fits.second->new_cells << " Matches:" << matches << " Min Anchor Diff:" << anchor_score << std::endl;
		if (matches > highest_match_count)
        {			
            best_score = best_fits.second->coverage;
            best_fit = idx;
			highest_match_count = matches;
        }

		if  (anchor_score < smallest_anchor_diff)
		{
			smallest_anchor_diff = anchor_score;
			best_score_a =  best_fits.second->coverage;
			best_anchor_diff_idx = idx;
		}
        idx++;
    }

	if (best_anchor_diff_idx != best_fit)
	{
		if (anchor_diffs.at(best_anchor_diff_idx) < 8 && anchor_diffs.at(best_fit) > 16)
		{
			best_fit = best_anchor_diff_idx;
		}
	}	

	history->DeleteHistoryUnitsExceptForFdg(best_fits_per_anchor_segment.at(best_fit).second->grid_idx, model_id);

    if (best_fits_per_anchor_segment.size() == 0) 
    { 
        std::cout << "BEST FIT ANCHOR SEGMENT SIZE == 0" << std::endl;  
        std::pair<std::vector<int>, FeatureDescStructureGraph::VG_stats*> nullResult;
        return nullResult;
    } 

    return best_fits_per_anchor_segment.at(best_fit);
}

//Search for segments which can fit this PCS description, create solution mappings and attach to solution node.
int PointCloudSceneSolution::SearchObjectsInScene(PointCloudSegmentation * objectPCS, SolutionNode * _parentSolution)
{	
    
    return 0;
}

//Greedy algorithm to determine whether boundaries exist in this scene. Returns most likely boundary walls.
//Assume there is only ONE floor in the scene. Possibly return multiple solutions
int PointCloudSceneSolution::SearchBoundaries(SolutionNode * _parent, float p_over_under_ratio, float p_min_reward, float p_reward_step)
{	
    int solutions_count=0;

    std::stringstream mess;
    std::stringstream mess2;	
    StructureGraph * structureGraph = m_sceneToSolve->m_structureGraph;
    SegmentsGraph * segmentsGraph = m_sceneToSolve->m_segmentsGraph;
    StructureGraph::Node * crt_node;
    StructureGraph * crt_internal_graph;
    StructureGraph::Node * crt_internal_node;
    SegmentsGraph::SurfaceSegment * crt_internal_segment;	
        
    Plane crt_node_plane;
    float crt_dst;	

    std::vector<std::pair<int, int>> segmentsNoUnder;
    std::vector<std::pair<int, int>> segmentsNoOver;

    int min_points_in_boundary_segment = 1000000;
    int max_points_in_boundary_segment = 0;

    mess.str(std::string(""));
    mess << "Determining Nodes with Potential for Boundary: " << std::endl;
    for (structureGraph->m_nodesIterator = structureGraph->m_nodes->begin(); structureGraph->m_nodesIterator != structureGraph->m_nodes->end(); structureGraph->m_nodesIterator++)
    {		
        crt_node = structureGraph->m_nodesIterator->second;		
        if (structureGraph->IsNodeMerged(crt_node->id)) continue; //skip if this is a merged node. 
        
        if (crt_node->stats->shape == StructureGraph::shape::plane)
        {			
            //compute over and under ratio
            mess2.str(std::string(""));
            std::pair<Vector3, std::pair<float,float>> ratio = m_sceneToSolve->ComputeRatioOfPointsAboveAndUnder(crt_node->id, 0);
            int node_size = crt_node->segment->points->size();
            if (m_sceneToSolve->m_structureGraph->m_mergedNodesSeeds->find(crt_node->id) != m_sceneToSolve->m_structureGraph->m_mergedNodesSeeds->end())
            {
                for (auto n_ids : m_sceneToSolve->m_structureGraph->m_mergedNodesSeeds->at(crt_node->id))
                {
                    node_size+=(structureGraph->m_nodes->at(n_ids))->segment->points->size();
                }
            }
            mess2 << "Plane Over/Under Ratio [" << crt_node->id << "]\tSize:" <<  node_size << " Normal:" << ratio.first.ToString() << "Over=" << ratio.second.first << " Under=" << ratio.second.second;
            (*(m_sceneToSolve->m_pEnvironment->GetLogger()))["Searching"]->Write(mess2.str(), LL_Error);
            if (ratio.second.first > p_over_under_ratio)
            {
                segmentsNoUnder.push_back(std::pair<int,int>(crt_node->id, node_size));
                if (node_size > max_points_in_boundary_segment) max_points_in_boundary_segment = node_size;
                if (node_size < min_points_in_boundary_segment) min_points_in_boundary_segment = node_size;
            }
            if (ratio.second.second > p_over_under_ratio)
            {
                segmentsNoOver.push_back(std::pair<int,int>(crt_node->id, node_size));
                if (node_size > max_points_in_boundary_segment) max_points_in_boundary_segment = node_size;
                if (node_size < min_points_in_boundary_segment) min_points_in_boundary_segment = node_size;
            }
        }				 
        
        if (crt_node->stats->shape == StructureGraph::shape::compound)
        {
            crt_internal_graph = crt_node->segment->internalStrGraph;
            for (auto internal_nodes_it = crt_internal_graph->m_nodes->begin(); internal_nodes_it != crt_internal_graph->m_nodes->end(); internal_nodes_it++)
            {
                crt_internal_node = internal_nodes_it->second;
                if (crt_internal_graph->IsNodeMerged(crt_internal_node->id)) continue;  //no need to check this node ... it has been merged.
                
                //compute over and under ratio				
                mess2.str(std::string(""));
                std::pair<Vector3, std::pair<float,float>> ratio = m_sceneToSolve->ComputeRatioOfPointsAboveAndUnder(crt_node->id, internal_nodes_it->first);
                mess2 << "Compound Over/Under Ratio [" << crt_node->id << "|" << crt_internal_node->id << "]\tSize:" <<  crt_internal_node->segment->points->size() << " Normal:" << ratio.first.ToString() << "Over=" << ratio.second.first << " Under=" << ratio.second.second;
                (*(m_sceneToSolve->m_pEnvironment->GetLogger()))["Searching"]->Write(mess2.str(), LL_Error);
                if (ratio.second.first > p_over_under_ratio) segmentsNoUnder.push_back(std::pair<int,int>(crt_node->id, crt_internal_node->segment->points->size()));
                if (ratio.second.second > p_over_under_ratio) segmentsNoOver.push_back(std::pair<int,int>(crt_node->id, crt_internal_node->segment->points->size()));
            }
        }
    }
    
    //Now determine according to reward which of these are boundaries. Obviously we can only use heuristics.	
    std::map<float, std::vector<StructureGraph::Node *>> orderedBoundaries; //  Ordered by reward. i.e. by how much is the size of cloud reduced + how many connections are removed (declared null) in the next global state of the MDP
    std::vector<std::pair<float, StructureGraph::Node *>> tmpBoundariesList;

    //Determine candidates.
    StructureGraph::Node * tmpNode;
    mess.str(std::string("")); mess << "Segments with No Points Above: ";
    for (int i=0; i<segmentsNoOver.size(); i++)
    {
        mess << segmentsNoOver[i].first << "[" << segmentsNoOver[i].second << "], ";
        //Compute reward for chosing this segment as a floor.
        tmpNode = structureGraph->m_nodes->at(segmentsNoOver[i].first);
        float size_score = (float)segmentsNoOver[i].second / (float)max_points_in_boundary_segment;
        float conns_dropped_score = (float)tmpNode->edges->size() / (float)_parent->crtActiveEdgesCount;
        orderedBoundaries[size_score+conns_dropped_score].push_back(tmpNode);
        tmpBoundariesList.push_back(std::pair<float, StructureGraph::Node *>(size_score+conns_dropped_score, tmpNode));
    }
    (*(m_sceneToSolve->m_pEnvironment->GetLogger()))["Searching"]->Write(mess.str(), LL_Error);
    
    mess.str(std::string("")); mess << "Segments with No Points Under: ";
    for (int i=0; i<segmentsNoUnder.size(); i++) 
    {		
        mess << segmentsNoUnder[i].first << "[" << segmentsNoUnder[i].second << "], ";
        //Compute reward for chosing this segment as a floor. 
        tmpNode = structureGraph->m_nodes->at(segmentsNoUnder[i].first);
        float size_score = (float)segmentsNoUnder[i].second / (float)max_points_in_boundary_segment;
        float conns_dropped_score = (float)tmpNode->edges->size() / (float)_parent->crtActiveEdgesCount;
        orderedBoundaries[size_score+conns_dropped_score].push_back(tmpNode);
        tmpBoundariesList.push_back(std::pair<float, StructureGraph::Node *>(size_score+conns_dropped_score, tmpNode));
    }
    (*(m_sceneToSolve->m_pEnvironment->GetLogger()))["Searching"]->Write(mess.str(), LL_Error);
    
    //Group candidates if coplanar i.e. all segments in the coplanar group will be considered as one boundary
    StructureGraph::Node * tmpNode2;
    std::map<int,int> ingroup;
    std::vector<std::pair<float,StructureGraph::Node *>> group;
    for (int i=0; i<tmpBoundariesList.size(); i++)
    {	
        if (ingroup.find(i) != ingroup.end()) continue; //skip this node .. already in group.
        
        float nodeiScore = tmpBoundariesList.at(i).first;
        float newiScore = nodeiScore;
        tmpNode = tmpBoundariesList.at(i).second;		
        group.clear();

        for (int j=0; j<tmpBoundariesList.size(); j++)
        {		
            if (i==j) continue;			
            tmpNode2 = tmpBoundariesList.at(j).second;		
            if (tmpNode->segment->isCoplanar(1.3f, tmpNode2->segment)) 
            {
                //These are nodes of coplanar surface segments, therefore JOIN
                group.push_back(std::pair<float, StructureGraph::Node*>(tmpBoundariesList.at(j).first,tmpNode2));
                newiScore+=tmpBoundariesList.at(j).first;
                ingroup[j] = i;
            } 
        }

        if (group.size() > 0) {
            //Group co-planar segments together.		
            orderedBoundaries[newiScore].push_back(tmpNode);
            for (int k=0; k<group.size(); k++)
            {				
                orderedBoundaries[newiScore].push_back(group.at(k).second);
                orderedBoundaries.erase(group.at(k).first);
            }			
            orderedBoundaries.erase(nodeiScore);
        }
    }

    std::vector<std::pair<float, std::vector<StructureGraph::Node *>>> orderedBoundariesList;
    std::vector<std::pair<float, std::vector<StructureGraph::Node *>>> chosen_boundaries;

    mess.str(std::string("")); mess << "-All Segments ordered by Score-";
    (*(m_sceneToSolve->m_pEnvironment->GetLogger()))["Searching"]->Write(mess.str(), LL_Error);
    float crt_reward_bracket_min = p_min_reward;
    float crt_reward_bracket_max = p_min_reward+p_reward_step;
    bool first_in_bracket = true;
    SolutionNode * child_sn;

    for (auto b_it = orderedBoundaries.begin(); b_it != orderedBoundaries.end(); b_it++) 
    {
        mess.str(std::string(""));
        mess << "Score[" << b_it->first << "] ";
        for (int i = 0; i<b_it->second.size(); i++)
        {
            mess << " ID::" << (b_it->second)[i]->id << " Size::" << (b_it->second)[i]->segment->points->size() << " - ";
        }
        (*(m_sceneToSolve->m_pEnvironment->GetLogger()))["Searching"]->Write(mess.str(), LL_Error);

        std::cout << crt_reward_bracket_min << "-" << crt_reward_bracket_max << std::endl;
        if (b_it->first < crt_reward_bracket_min) continue;
        std::cout << "NICE !!! " << b_it->first << " THIS IS A GOOD SCORE !!! " << std::endl;
        if (b_it->first > crt_reward_bracket_max) 
        {
            std::cout << "VERY GOOD SCORE !! " << b_it->first << " MOVING UP THE BRACKET " << std::endl;
            while (b_it->first - p_reward_step > crt_reward_bracket_min)
                crt_reward_bracket_min += p_reward_step;
            crt_reward_bracket_max = crt_reward_bracket_min + p_reward_step;
            first_in_bracket = true;
            std::cout << "NEW BRACKET !! " << crt_reward_bracket_min << " <-> " << crt_reward_bracket_max << std::endl;
        }

        if (first_in_bracket) 
        {
            first_in_bracket = false;			
            child_sn = m_markovDecisionProcess->CreateNode();
            _parent->AttachChild(child_sn);
            child_sn->m_parent = _parent;
        } 

        //Update Mapping in the crt solution node
        child_sn->AddObjectMatchInMapping("boundary", std::pair<float, std::vector<StructureGraph::Node *>> (b_it->first, b_it->second));
    }	
    
    //There's no need to determine floors and walls ... just set some segments as boundary segments those with a reward > 0.4f  and steps of 0.2f;
    return solutions_count;
}

PointCloudSceneSolution::SolutionMapping * PointCloudSceneSolution::GetPreferredSolution() 
{
    return m_preferred_solution;
}

void PointCloudSceneSolution::WriteSolutionToFile(std::map<int, std::vector<GraphCut*>*> * &cuts, const char * filename)
{
/*	std::ofstream cutsfile;
    cutsfile.open (filename, std::ios::trunc);
    int cut_count = 0;
    int total_cut_count = 0;
    for (std::map<int, std::vector<GraphCut*>*>::iterator miterator = cuts->begin(); miterator != cuts->end(); miterator++)
    {
        cutsfile << "*****************************************\n";
        cutsfile << "Valid Cuts From Node = " << miterator->first << "\n";
        for (std::vector<GraphCut*>::iterator cutsi = miterator->second->begin(); cutsi != miterator->second->end(); cutsi++)
        {	
            for (std::vector<int>::iterator oCNIterator = (*cutsi)->orderedCutNodes->begin(); oCNIterator != (*cutsi)->orderedCutNodes->end(); oCNIterator++)
            {
                cutsfile << *oCNIterator << " ";
            }			
            cutsfile << "  - #Transitions=" << (*cutsi)->transitionCount << " #Directions=" << (*cutsi)->DirectionsTravelled->size() << " #Score=" << (*cutsi)->score << "\n";
            cut_count++;
        }
        cutsfile << "*****************************************\n";
        cutsfile << "Total Cuts = " << cut_count << "\n";
        cutsfile << "*****************************************\n";
        total_cut_count += cut_count;
        cut_count = 0;
    }
    cutsfile << "Total Cuts in Scene = " << total_cut_count << "\n";
    cutsfile << "*****************************************" << std::endl;
    cutsfile.close();*/
}

void PointCloudSceneSolution::WriteIlluminPRTScript(std::string p_path)
{

}
                